/****************************************************************************************
**  File: Matrix4x4.hpp
**  Author: Asteroth
**  Date: 22/02/2009
****************************************************************************************/

#ifndef __MATRIX4X4_HPP__
#define __MATRIX4X4_HPP__

const char MATRIX_NOT_INIT = 0;
const char MATRIX_IDENTITY = 1;
const char MATRIX_NULL = 2;

template <class T>
class Matrix4x4x {
public:
	T M[16];

	// --- Constructors ----------------------------------------------------
	// special
	Matrix4x4x(char init = MATRIX_NOT_INIT);
	// copy
	Matrix4x4x(const Matrix4x4x<T> &mat);

	// --- Operators -------------------------------------------------------
	// =
	inline Matrix4x4x<T> &operator = (const Matrix4x4x<T> &mat);
	// multiplier  ^=  ^
	inline Matrix4x4x<T> &operator ^= (const Matrix4x4x<T> &mat);
	inline Matrix4x4x<T> operator ^ (const Matrix4x4x<T> &mat);

	// --- Functions -------------------------------------------------------
	inline void makeIdentity();
	inline void makeInverse();
	inline void makeScale(T x, T y, T z);
	inline void makeScale(const Vector3x<T> &scale);
	inline void makeTranslation(T x, T y, T z);
	inline void makeTranslation(const Vector3x<T> &translation);
	inline void makeRotation(const Vector3x<T> &arb, float angle);
	inline void makeRotationX(float angle);
	inline void makeRotationY(float angle);
	inline void makeRotationZ(float angle);
	inline void makeTransformation(const Vector3x<T> &xdir, const Vector3x<T> &ydir, const Vector3x<T> &zdir);
	inline void makeTransformation(const Vector3x<T> &xdir, const Vector3x<T> &ydir, const Vector3x<T> &zdir, const Vector3x<T> &pos);
	inline void makeFrustum(const float left, const float right, const float bottom, const float top, const float znear, const float zfar);
	inline void makeLookAt(const Vector3x<T> &position, const Vector3x<T> &target, const Vector3x<T> &upVec);
	inline void makeOrtho(float left, float right, float top, float bottom, float znear, float zfar);
	inline void makeView2D(float xorigin, float yorigin, float width, float height, float znear = -1.0, float zfar = 100.0);
	inline void makeView3D(const Vector3x<T> &position, const Vector3x<T> &target, const Vector3x<T> &upVec = Vector3x<T>(0.0, 0.0, 1.0), float aspectRatio = 1.333, float viewFactor = 1.0, float znear = 1.0, float zfar = 1000.0);

	inline void makeEulerAngles(float pitch, float yaw, float roll);
	inline void makeQuaternion(const Quaternionx<T> &qt);

	static inline Matrix4x4x<T> createIdentity();
	static inline Matrix4x4x<T> createInverse(const Matrix4x4x &mat);
	static inline Matrix4x4x<T> createScale(T x, T y, T z);
	static inline Matrix4x4x<T> createScale(const Vector3x<T> &scale);
	static inline Matrix4x4x<T> createTranslation(T x, T y, T z);
	static inline Matrix4x4x<T> createTranslation(const Vector3x<T> &translation);
	static inline Matrix4x4x<T> createRotation(const Vector3x<T> &arb, float angle);
	static inline Matrix4x4x<T> createRotationX(float angle);
	static inline Matrix4x4x<T> createRotationY(float angle);
	static inline Matrix4x4x<T> createRotationZ(float angle);
	static inline Matrix4x4x<T> createTransformation(const Vector3x<T> &xdir, const Vector3x<T> &ydir, const Vector3x<T> &zdir);
	static inline Matrix4x4x<T> createTransformation(const Vector3x<T> &xdir, const Vector3x<T> &ydir, const Vector3x<T> &zdir, const Vector3x<T> &pos);
	static inline Matrix4x4x<T> createFrustum(const float left, const float right, const float bottom, const float top, const float znear, const float zfar);
	static inline Matrix4x4x<T> createLookAt(const Vector3x<T> &position, const Vector3x<T> &target, const Vector3x<T> &upVec);
	static inline Matrix4x4x<T> createOrtho(float left, float right, float top, float bottom, float znear, float zfar);
	static inline Matrix4x4x<T> createView2D(float xorigin, float yorigin, float width, float height, float znear = -1.0, float zfar = 100.0);
	static inline Matrix4x4x<T> createView3D(const Vector3x<T> &position, const Vector3x<T> &target, const Vector3x<T> &upVec = Vector3x<T>(0.0, 0.0, 1.0), float aspectRatio = 1.333, float viewFactor = 1.0, float znear = 1.0, float zfar = 1000.0);

	static inline Matrix4x4x<T> createEulerAngles(float pitch, float yaw, float roll);
	static inline Matrix4x4x<T> createQuaternion(const Quaternionx<T> &qt);

	inline Quaternionx<T> toQuaternion() const;
};

typedef Matrix4x4x<float> Matrix4x4;

const Matrix4x4 IDENTITY_MATRIX = Matrix4x4(MATRIX_IDENTITY);
const Matrix4x4 ZERO_MATRIX = Matrix4x4(MATRIX_NULL);

const Matrix4x4 ROTX090_MATRIX = Matrix4x4::createRotationX(PI*0.5f);
const Matrix4x4 ROTX180_MATRIX = Matrix4x4::createRotationX(PI*1.0f);
const Matrix4x4 ROTX270_MATRIX = Matrix4x4::createRotationX(PI*1.5f);

const Matrix4x4 ROTY090_MATRIX = Matrix4x4::createRotationY(PI*0.5f);
const Matrix4x4 ROTY180_MATRIX = Matrix4x4::createRotationY(PI*1.0f);
const Matrix4x4 ROTY270_MATRIX = Matrix4x4::createRotationY(PI*1.5f);

const Matrix4x4 ROTZ090_MATRIX = Matrix4x4::createRotationZ(PI*0.5f);
const Matrix4x4 ROTZ180_MATRIX = Matrix4x4::createRotationZ(PI*1.0f);
const Matrix4x4 ROTZ270_MATRIX = Matrix4x4::createRotationZ(PI*1.5f);


// Helper functions
static inline void ____swapRows(GLfloat mat[4][8], int r1, int r2)
{
    GLfloat tmp;

    tmp = mat[r1][0];   mat[r1][0] = mat[r2][0];    mat[r2][0] = tmp;
    tmp = mat[r1][1];   mat[r1][1] = mat[r2][1];    mat[r2][1] = tmp;
    tmp = mat[r1][2];   mat[r1][2] = mat[r2][2];    mat[r2][2] = tmp;
    tmp = mat[r1][3];   mat[r1][3] = mat[r2][3];    mat[r2][3] = tmp;
    tmp = mat[r1][4];   mat[r1][4] = mat[r2][4];    mat[r2][4] = tmp;
    tmp = mat[r1][5];   mat[r1][5] = mat[r2][5];    mat[r2][5] = tmp;
    tmp = mat[r1][6];   mat[r1][6] = mat[r2][6];    mat[r2][6] = tmp;
    tmp = mat[r1][7];   mat[r1][7] = mat[r2][7];    mat[r2][7] = tmp;
}
static void ____makeLadderMatrix(GLfloat mat[4][8], int depth)
{
    int i;
    for (i = depth; i < 4; i++)
        if (mat[i][depth])
            break;

    if (i != 4) {
        if (i != depth)
            ____swapRows( mat, i, depth );

        for (i = depth+1; i < 4; i++) {
            if (mat[i][depth]) {
                double multiplier = -(mat[i][depth] / mat[depth][depth]);

                for (short j = depth; j < 8; j++) {
                    mat[depth][j] *= multiplier;
                    mat[i][j] += mat[depth][j];
                }

                mat[i][depth] = 0;
            }
        }
    }

    if (depth<3)
        ____makeLadderMatrix(mat, depth+1);
}
static void ____makeOnlyPivotLeft(GLfloat mat[4][8], int depth)
{
    int i;
    for (i = depth; i >= 0; i--)
        if ( mat[i][depth])
            break;

    if (i!=-1)
    {
        for (i=depth-1; i>=0; i--)
        {
            if (mat[i][depth])
            {
                double multiplier = -(mat[i][depth] / mat[depth][depth]);

                for (int j=depth; j<8; j++)
                {
                    mat[depth][j] *= multiplier;
                    mat[i][j] += mat[depth][j];
                }

                mat[i][depth] = 0;
            }
        }
    }

    if (depth!=0)
        ____makeOnlyPivotLeft(mat, depth-1);
}
static void ____makeIdentityMatrix(GLfloat mat[4][8], int depth)
{
    if (mat[depth][depth])
    {
        double multiplier = (1.0 / mat[depth][depth]);

        for (int j=depth; j<8; j++)
            mat[depth][j] *= multiplier;
    }

    if (depth<3)
        ____makeIdentityMatrix(mat, depth+1);
}

// Constructors
template <class T> Matrix4x4x<T>::Matrix4x4x( char init )
{
    if ( init == MATRIX_NOT_INIT )
        return;
    else if ( init == MATRIX_IDENTITY )
    {
        M[0] = M[5] = M[10] = M[15] = (T)1.0;
        M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = (T)0.0;
    }
    else if ( init == MATRIX_NULL )
    {
        M[0] = M[1] = M[2] = M[3] = M[4] = M[5] = M[6] = M[7] = M[8] = M[9] = M[10] = M[11] = M[12] = M[13] = M[14] = M[15] = (T)0.0;
    }
}
template <class T> Matrix4x4x<T>::Matrix4x4x( const Matrix4x4x<T> &mat )
{
    M[0] = mat.M[0];    M[4] = mat.M[4];    M[8] = mat.M[8];    M[12] = mat.M[12];
    M[1] = mat.M[1];    M[5] = mat.M[5];    M[9] = mat.M[9];    M[13] = mat.M[13];
    M[2] = mat.M[2];    M[6] = mat.M[6];    M[10] = mat.M[10];  M[14] = mat.M[14];
    M[3] = mat.M[3];    M[7] = mat.M[7];    M[11] = mat.M[11];  M[15] = mat.M[15];
}

// Operators
template <class T> inline Matrix4x4x<T> &Matrix4x4x<T>::operator = ( const Matrix4x4x<T> &mat )
{
    M[0] = mat.M[0];    M[4] = mat.M[4];    M[8] = mat.M[8];    M[12] = mat.M[12];
    M[1] = mat.M[1];    M[5] = mat.M[5];    M[9] = mat.M[9];    M[13] = mat.M[13];
    M[2] = mat.M[2];    M[6] = mat.M[6];    M[10] = mat.M[10];  M[14] = mat.M[14];
    M[3] = mat.M[3];    M[7] = mat.M[7];    M[11] = mat.M[11];  M[15] = mat.M[15];
    return *this;
}
template <class T> inline Matrix4x4x<T> &Matrix4x4x<T>::operator ^= ( const Matrix4x4x<T> &mat )
{
    Matrix4x4x<T> m1(*this);
	M[0] =  m1.M[0]*mat.M[0] +  m1.M[4]*mat.M[1] +  m1.M[8]*mat.M[2] +   m1.M[12]*mat.M[3];
	M[1] =  m1.M[1]*mat.M[0] +  m1.M[5]*mat.M[1] +  m1.M[9]*mat.M[2] +   m1.M[13]*mat.M[3];
	M[2] =  m1.M[2]*mat.M[0] +  m1.M[6]*mat.M[1] +  m1.M[10]*mat.M[2] +  m1.M[14]*mat.M[3];
	M[3] =  m1.M[3]*mat.M[0] +  m1.M[7]*mat.M[1] +  m1.M[11]*mat.M[2] +  m1.M[15]*mat.M[3];
	M[4] =  m1.M[0]*mat.M[4] +  m1.M[4]*mat.M[5] +  m1.M[8]*mat.M[6] +   m1.M[12]*mat.M[7];
	M[5] =  m1.M[1]*mat.M[4] +  m1.M[5]*mat.M[5] +  m1.M[9]*mat.M[6] +   m1.M[13]*mat.M[7];
	M[6] =  m1.M[2]*mat.M[4] +  m1.M[6]*mat.M[5] +  m1.M[10]*mat.M[6] +  m1.M[14]*mat.M[7];
	M[7] =  m1.M[3]*mat.M[4] +  m1.M[7]*mat.M[5] +  m1.M[11]*mat.M[6] +  m1.M[15]*mat.M[7];
	M[8] =  m1.M[0]*mat.M[8] +  m1.M[4]*mat.M[9] +  m1.M[8]*mat.M[10] +  m1.M[12]*mat.M[11];
	M[9] =  m1.M[1]*mat.M[8] +  m1.M[5]*mat.M[9] +  m1.M[9]*mat.M[10] +  m1.M[13]*mat.M[11];
	M[10] = m1.M[2]*mat.M[8] +  m1.M[6]*mat.M[9] +  m1.M[10]*mat.M[10] + m1.M[14]*mat.M[11];
	M[11] = m1.M[3]*mat.M[8] +  m1.M[7]*mat.M[9] +  m1.M[11]*mat.M[10] + m1.M[15]*mat.M[11];
	M[12] = m1.M[0]*mat.M[12] + m1.M[4]*mat.M[13] + m1.M[8]*mat.M[14] +  m1.M[12]*mat.M[15];
	M[13] = m1.M[1]*mat.M[12] + m1.M[5]*mat.M[13] + m1.M[9]*mat.M[14] +  m1.M[13]*mat.M[15];
	M[14] = m1.M[2]*mat.M[12] + m1.M[6]*mat.M[13] + m1.M[10]*mat.M[14] + m1.M[14]*mat.M[15];
	M[15] = m1.M[3]*mat.M[12] + m1.M[7]*mat.M[13] + m1.M[11]*mat.M[14] + m1.M[15]*mat.M[15];
	return *this;
}
template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::operator ^ ( const Matrix4x4x<T> &mat )
{
    Matrix4x4x<T> out;
	out.M[0] =  M[0]*mat.M[0] +  M[4]*mat.M[1] +  M[8]*mat.M[2] +   M[12]*mat.M[3];
	out.M[1] =  M[1]*mat.M[0] +  M[5]*mat.M[1] +  M[9]*mat.M[2] +   M[13]*mat.M[3];
	out.M[2] =  M[2]*mat.M[0] +  M[6]*mat.M[1] +  M[10]*mat.M[2] +  M[14]*mat.M[3];
	out.M[3] =  M[3]*mat.M[0] +  M[7]*mat.M[1] +  M[11]*mat.M[2] +  M[15]*mat.M[3];
	out.M[4] =  M[0]*mat.M[4] +  M[4]*mat.M[5] +  M[8]*mat.M[6] +   M[12]*mat.M[7];
	out.M[5] =  M[1]*mat.M[4] +  M[5]*mat.M[5] +  M[9]*mat.M[6] +   M[13]*mat.M[7];
	out.M[6] =  M[2]*mat.M[4] +  M[6]*mat.M[5] +  M[10]*mat.M[6] +  M[14]*mat.M[7];
	out.M[7] =  M[3]*mat.M[4] +  M[7]*mat.M[5] +  M[11]*mat.M[6] +  M[15]*mat.M[7];
	out.M[8] =  M[0]*mat.M[8] +  M[4]*mat.M[9] +  M[8]*mat.M[10] +  M[12]*mat.M[11];
	out.M[9] =  M[1]*mat.M[8] +  M[5]*mat.M[9] +  M[9]*mat.M[10] +  M[13]*mat.M[11];
	out.M[10] = M[2]*mat.M[8] +  M[6]*mat.M[9] +  M[10]*mat.M[10] + M[14]*mat.M[11];
	out.M[11] = M[3]*mat.M[8] +  M[7]*mat.M[9] +  M[11]*mat.M[10] + M[15]*mat.M[11];
	out.M[12] = M[0]*mat.M[12] + M[4]*mat.M[13] + M[8]*mat.M[14] +  M[12]*mat.M[15];
	out.M[13] = M[1]*mat.M[12] + M[5]*mat.M[13] + M[9]*mat.M[14] +  M[13]*mat.M[15];
	out.M[14] = M[2]*mat.M[12] + M[6]*mat.M[13] + M[10]*mat.M[14] + M[14]*mat.M[15];
	out.M[15] = M[3]*mat.M[12] + M[7]*mat.M[13] + M[11]*mat.M[14] + M[15]*mat.M[15];
	return out;
}

// Functions
template <class T> inline void Matrix4x4x<T>::makeIdentity()
{
    M[0] = M[5] = M[10] = M[15] = (T)1.0;
    M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = (T)0.0;
}

template <class T> inline void Matrix4x4x<T>::makeInverse()
{
    T cMat[4][8] = {
        {M[0],  M[1],  M[2],  M[3],  1, 0, 0, 0},
        {M[4],  M[5],  M[6],  M[7],  0, 1, 0, 0},
        {M[8],  M[9],  M[10], M[11], 0, 0, 1, 0},
        {M[12], M[13], M[14], M[15], 0, 0, 0, 1}
        };

    ____makeLadderMatrix( cMat, 0 );

    ____makeOnlyPivotLeft( cMat, 3 );

    ____makeIdentityMatrix( cMat, 0 );

    for ( char i = 0; i < 16; i++ )
        M[i] = cMat[char(i/4)][i%4+4];
}

template <class T> inline void Matrix4x4x<T>::makeScale( T x, T y, T z )
{
    M[0] = x;
    M[5] = y;
    M[10] = z;
    M[15] = (T)1.0;
    M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = (T)0.0;
}

template <class T> inline void Matrix4x4x<T>::makeScale( const Vector3x<T> &scale )
{
    M[0] = scale.X;
    M[5] = scale.Y;
    M[10] = scale.Z;
    M[15] = (T)1.0;
    M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = (T)0.0;
}

template <class T> inline void Matrix4x4x<T>::makeTranslation( T x, T y, T z )
{
    M[0] = M[5] = M[10] = M[15] = (T)1.0;
    M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = (T)0.0;
    M[12] = x;
    M[13] = y;
    M[14] = z;
}

template <class T> inline void Matrix4x4x<T>::makeTranslation( const Vector3x<T> &translation )
{
    M[0] = M[5] = M[10] = M[15] = (T)1.0;
    M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = (T)0.0;
    M[12] = translation.X;
    M[13] = translation.Y;
    M[14] = translation.Z;
}

template <class T> inline void Matrix4x4x<T>::makeRotation( const Vector3x<T> &arb, float angle )
{
    float s = sin(angle);
    float c = cos(angle);
    float t = 1.0-cos(angle);

    M[0] = t * arb.X * arb.X + c;
    M[1] = t * arb.X * arb.Y - s * arb.Z;
    M[2] = t * arb.X * arb.Y + s * arb.Y;
    M[3] = 0.0;
    M[4] = t * arb.X * arb.Y + s * arb.Z;
    M[5] = t * arb.Y * arb.Y + c;
    M[6] = t * arb.Y * arb.Z - s * arb.X;
    M[7] = 0.0;
    M[8] = t * arb.X * arb.Y - s * arb.Y;
    M[9] = t * arb.Y * arb.Z + s * arb.X;
    M[10] = t * arb.Z * arb.Z + c;
    M[11] = M[12] = M[13] = M[14] = 0.0;
    M[15] = 1.0;
}

template <class T> inline void Matrix4x4x<T>::makeRotationX( float angle )
{
    float ca = cos( angle );
    float sa = sin( angle );

    M[5] = ca;
    M[6] = sa;
    M[9] = -sa;
    M[10] = ca;

    M[1] = M[2] = M[3] = M[4] = M[7] = M[8] = M[11] = M[12] = M[13] = M[14] = 0.0;
    M[0] = M[15] = 1.0;
}

template <class T> inline void Matrix4x4x<T>::makeRotationY( float angle )
{
    float ca = cos( angle );
    float sa = sin( angle );

    M[0] = ca;
    M[2] = -sa;
    M[8] = sa;
    M[10] = ca;

    M[1] = M[3] = M[4] = M[6] = M[7] = M[9] = M[11] = M[12] = M[13] = M[14] = 0.0;
    M[5] = M[15] = 1.0;
}

template <class T> inline void Matrix4x4x<T>::makeRotationZ( float angle )
{
    float ca = cos( angle );
    float sa = sin( angle );

    M[0] = ca;
    M[1] = sa;
    M[4] = -sa;
    M[5] = ca;

    M[2] = M[3] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = 0.0;
    M[10] = M[15] = 1.0;
}

template <class T> inline void Matrix4x4x<T>::makeTransformation(
    const Vector3x<T> &xdir,
    const Vector3x<T> &ydir,
    const Vector3x<T> &zdir)
{
	M[0] = xdir.X;
	M[1] = ydir.X;
	M[2] = zdir.X;
	M[3] = 0.0;

	M[4] = xdir.Y;
	M[5] = ydir.Y;
	M[6] = zdir.Y;
	M[7] = 0.0;

	M[8] = xdir.Z;
	M[9] = ydir.Z;
	M[10] = zdir.Z;
	M[11] = 0.0;

	M[12] = 0.0;
	M[13] = 0.0;
	M[14] = 0.0;
	M[15] = 1.0;
}

template <class T> inline void Matrix4x4x<T>::makeTransformation(
    const Vector3x<T> &xdir,
    const Vector3x<T> &ydir,
    const Vector3x<T> &zdir,
    const Vector3x<T> &pos)
{
	M[0] = xdir.X;
	M[1] = ydir.X;
	M[2] = zdir.X;
	M[3] = 0.0;

	M[4] = xdir.Y;
	M[5] = ydir.Y;
	M[6] = zdir.Y;
	M[7] = 0.0;

	M[8] = xdir.Z;
	M[9] = ydir.Z;
	M[10] = zdir.Z;
	M[11] = 0.0;

	M[12] = pos.x;
	M[13] = pos.y;
	M[14] = pos.z;
	M[15] = 1.0;
}

template <class T> inline void Matrix4x4x<T>::makeFrustum(const float left, const float right, const float bottom, const float top, const float znear, const float zfar)
{
    M[0] = (znear * 2.0) / (right - left);
    M[5] = (znear * 2.0) / (top - bottom);
    M[8] = (right + left) / (right - left);
    M[9] = (top + bottom) / (top - bottom);
    M[10] = (zfar + znear) / (znear - zfar);
    M[14] = (2.0 * zfar * znear) / (znear - zfar);
    M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[12] = M[13] = M[15] = 0.0;
    M[11] = -1.0;
}

template <class T> inline void Matrix4x4x<T>::makeLookAt(const Vector3x<T> &position, const Vector3x<T> &target, const Vector3x<T> &upVec)
{
    Vector3x<T> f, r, u;
    Matrix4x4x<T> m;

    f = !(position - target);
    r = !(upVec && f);
    u = !(f && r);

    makeTransformation(r, u, f);
    m.makeTranslation(-position);
    *this ^= m;
}

template <class T> inline void Matrix4x4x<T>::makeOrtho(float left, float right, float top, float bottom, float znear, float zfar)
{
	M[0]=2.0/(right-left);
	M[5]=2.0/(top-bottom);
	M[10]=-2.0/(zfar-znear);
	M[12]=-(right+left)/(right-left);
	M[13]=-(top+bottom)/(top-bottom);
	M[14]=-(zfar+znear)/(zfar-znear);
	M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = 0.0f;
	M[15] = 1.0f;
}

template <class T> inline void Matrix4x4x<T>::makeView2D(float xorigin, float yorigin, float width, float height, float znear, float zfar)
{
	M[0]=2.0/((xorigin+width-0.5)-(xorigin-0.5));
	M[5]=2.0/((yorigin-0.5)-(yorigin+height-0.5));
	M[10]=-2.0/(zfar-znear);
	M[12]=-((xorigin+width-0.5)+(xorigin-0.5))/((xorigin+width-0.5)-(xorigin-0.5));
	M[13]=-((yorigin-0.5)+(yorigin+height-0.5))/((yorigin-0.5)-(yorigin+height-0.5));
	M[14]=-(zfar+znear)/(zfar-znear);
	M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = 0.0f;
	M[15] = 1.0f;
}

template <class T> inline void Matrix4x4x<T>::makeView3D(const Vector3x<T> &position, const Vector3x<T> &target, const Vector3x<T> &upVec, float aspectRatio, float viewFactor, float znear, float zfar)
{
	*this = createFrustum(-aspectRatio*viewFactor*0.5, aspectRatio*viewFactor*0.5, -viewFactor*0.5, viewFactor*0.5, znear, zfar) ^ createLookAt(position, target, upVec);
}

template <class T> inline void Matrix4x4x<T>::makeEulerAngles(float pitch, float yaw, float roll)
{
    float xc = cos(pitch);
    float xs = sin(pitch);
    float yc = cos(yaw);
    float ys = sin(yaw);
    float zc = cos(roll);
    float zs = sin(roll);

    float xcys = xc * ys;
    float xsys = xs * ys;

    M[0] = yc * zc;
    M[1] = -yc * zs;
    M[2] = -ys;

    M[4] = -xsys * zc + xc * zs;
    M[5] = xsys * zs + xc * zc;
    M[6] = -xs * yc;

    M[8] = xcys * zc + xs * zs;
    M[9] = -xcys * zs + xs * zc;
    M[10]= xc * yc;

    M[3] =  M[7] = M[11] = M[12] = M[13] = M[14] = 0.0;
    M[15] =  1.0;
}

template <class T> inline void Matrix4x4x<T>::makeQuaternion(const Quaternionx<T> &qt)
{
    float xx = qt.X * qt.X;
    float xy = qt.X * qt.Y;
    float xz = qt.X * qt.Z;
    float xw = qt.X * qt.W;
    float yy = qt.Y * qt.Y;
    float yz = qt.Y * qt.Z;
    float yw = qt.Y * qt.W;
    float zz = qt.Z * qt.Z;
    float zw = qt.Z * qt.W;

    M[0] = 1.0 - 2.0 * ( yy + zz );
    M[1] =       2.0 * ( xy - zw );
    M[2] =       2.0 * ( xz + yw );

    M[4] =       2.0 * ( xy + zw );
    M[5] = 1.0 - 2.0 * ( xx + zz );
    M[6] =       2.0 * ( yz - xw );

    M[8] =       2.0 * ( xz - yw );
    M[9] =       2.0 * ( yz + xw );
    M[10]= 1.0 - 2.0 * ( xx + yy );

    M[3]  = M[7] = M[11] = M[12] = M[13] = M[14] = 0;
    M[15] = 1.0;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createIdentity()
{
    return IDENTITY_MATRIX;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createInverse(const Matrix4x4x &mat)
{
    Matrix4x4x<T> out(mat);
    out.makeInverse();
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createScale(T x, T y, T z)
{
    Matrix4x4x<T> out;
    out.makeScale(x, y, z);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createScale(const Vector3x<T> &scale)
{
    Matrix4x4x<T> out;
    out.makeScale(scale);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createTranslation(T x, T y, T z)
{
    Matrix4x4x<T> out;
    out.makeTranslation(x, y, z);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createTranslation(const Vector3x<T> &translation)
{
    Matrix4x4x<T> out;
    out.makeTranslation(translation);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createRotation(const Vector3x<T> &arb, float angle)
{
    Matrix4x4x<T> out;
    out.makeRotation(arb, angle);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createRotationX(float angle)
{
    Matrix4x4x<T> out;
    out.makeRotationX(angle);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createRotationY(float angle)
{
    Matrix4x4x<T> out;
    out.makeRotationY(angle);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createRotationZ(float angle)
{
    Matrix4x4x<T> out;
    out.makeRotationZ(angle);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createTransformation(const Vector3x<T> &xdir, const Vector3x<T> &ydir, const Vector3x<T> &zdir)
{
    Matrix4x4x<T> out;
    out.makeTransformation(xdir, ydir, zdir);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createFrustum(const float left, const float right, const float bottom, const float top, const float znear, const float zfar)
{
    Matrix4x4x<T> out;
    out.makeFrustum(left, right, bottom, top, znear, zfar);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createLookAt(const Vector3x<T> &position, const Vector3x<T> &target, const Vector3x<T> &upVec)
{
    Matrix4x4x<T> out;
    out.makeLookAt(position, target, upVec);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createOrtho(float left, float right, float top, float bottom, float znear, float zfar)
{
    Matrix4x4x<T> out;
    out.makeOrtho(left, right, top, bottom, znear, zfar);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createView2D(float xorigin, float yorigin, float width, float height, float znear, float zfar)
{
    Matrix4x4x<T> out;
    out.makeView2D(xorigin, yorigin, width, height, znear, zfar);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createView3D(const Vector3x<T> &position, const Vector3x<T> &target, const Vector3x<T> &upVec, float aspectRatio, float viewFactor, float znear, float zfar)
{
    Matrix4x4x<T> out;
    out.makeView3D(position, target, upVec, aspectRatio, viewFactor, znear, zfar);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createEulerAngles(float pitch, float yaw, float roll)
{
    Matrix4x4x<T> out;
    out.makeEulerAngles(pitch, yaw, roll);
    return out;
}

template <class T> inline Matrix4x4x<T> Matrix4x4x<T>::createQuaternion(const Quaternionx<T> &qt)
{
    Matrix4x4x<T> out;
    out.makeQuaternion(qt);
    return out;
}

template <class T> inline Quaternionx<T> Matrix4x4x<T>::toQuaternion() const
{
    float t = M[0] + M[5] + M[10] + 1.0;

    if (t > 0)
    {
        float s = 0.50 / sqrt(t);
        float w = 0.25 / s;

        return Quaternionx<T>(
            (M[9] - M[6]) * s,
            (M[2] - M[8]) * s,
            (M[4] - M[1]) * s,
            0.25 / s);
    }
    else
    {
        float s;
        float x, y, z, w;

        if (M[0] > M[5] && M[0] > M[10]) //column 1
        {
            s  = sqrt(1.0f + M[0] - M[5] - M[10]) * 2.0;

            x = 0.5 / s;
            y = (M[1] + M[4]) / s;
            z = (M[2] + M[8]) / s;
            w = (M[6] + M[9]) / s;
        }
        else if (M[5] > M[10])  //column 2
        {
            s  = sqrt(1.0f + M[5] - M[0] - M[10]) * 2.0;

            x = (M[1] + M[4]) / s;
            y = 0.5 / s;
            z = (M[6] + M[9]) / s;
            w = (M[2] + M[8]) / s;
        }
        else  //column 3
        {
            s  = sqrt(1.0 + M[10] - M[0] - M[5]) * 2.0;

            x = (M[2] + M[8]) / s;
            y = (M[6] + M[9]) / s;
            z = 0.5 / s;
            w = (M[1] + M[4]) / s;
        }

        return Quaternionx<T>(x, y, z, w);
    }
}

#endif
